//
// Created by zhanglei on 2023/12/18.
//
#include "common/sensor_calib.h"

namespace sad {

SensorCalib::SensorCalib() {
    imu_pose_in_vehicle_ = ComputeImuPose();
    gps_pose_in_vehicle_ = ComputeGpsPose();
    left_velodyne_pose_in_vehicle_ = ComputeLeftVelodynePose();

    left_velodyne_pose_in_imu_ = TransLidarToImu(imu_pose_in_vehicle_, left_velodyne_pose_in_vehicle_);
}

Eigen::Matrix4d SensorCalib::ComputeImuPose() {
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    pose(0, 3) = -0.07;
    pose(2, 3) = 1.7;
    return pose;
}

Eigen::Matrix4d SensorCalib::ComputeGpsPose() {
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    pose(0, 3) = -0.32;
    pose(2, 3) = 1.7;    
    return pose;
}

Eigen::Matrix4d SensorCalib::ComputeLeftVelodynePose() {
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    pose(0, 0) = -0.516377;
    pose(0, 1) = -0.702254;
    pose(0, 2) = -0.490096;
    pose(0, 3) = -0.334623;
    pose(1, 0) = 0.491997;
    pose(1, 1) = -0.711704;
    pose(1, 2) = 0.501414;
    pose(1, 3) = 0.431973;  
    pose(2, 0) = -0.700923;
    pose(2, 1) = 0.0177927;
    pose(2, 2) = 0.713015;  
    pose(2, 3) = 1.94043;
    return pose;    
}


SE3 SensorCalib::EigenToSE3(const Eigen::Matrix4d& pose) {
    Eigen::Matrix3d R = pose.block<3,3>(0,0);

    Eigen::Quaterniond rotation(R);
    rotation.normalize();

    Eigen::Vector3d t = pose.block<3,1>(0,3);
    return Sophus::SE3d(rotation.toRotationMatrix(), t);
}

Eigen::Matrix4d SensorCalib::SE3ToEigen(const SE3& pose) {
    Eigen::Matrix3d R = pose.so3().matrix();

    //std::cout << " R : " << R << std::endl;

    Eigen::Vector3d t = pose.translation();
    return RtToPose(R, t);
}

void SensorCalib::PoseToRt(Eigen::Matrix3d& R, Eigen::Vector3d& t, const Eigen::Matrix4d& pose) {
    R = pose.block<3,3>(0,0);
    t = pose.block<3,1>(0,3);
}

Eigen::Matrix4d SensorCalib::RtToPose(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) {
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    pose.block<3,3>(0,0) = R;
    pose.block<3,1>(0,3) = t;
    return pose;
}

Eigen::Matrix4d SensorCalib::TransLidarToImu(const Eigen::Matrix4d& imu_pose, const Eigen::Matrix4d& lidar_pose) {
    Eigen::Matrix4d lidar_pose_in_imu = imu_pose.inverse() * lidar_pose;
    
    std::cout << " lidar_pose_in_imu : " << lidar_pose_in_imu << std::endl;

    return lidar_pose_in_imu;
}

SE3 SensorCalib::ImuToLidarMotion(const SE3& imu_pose, const Eigen::Matrix4d& lidar_pose) {
    Eigen::Matrix4d lidar_pose_from_imu = lidar_pose * SE3ToEigen(imu_pose) * lidar_pose.inverse();
    //std::cout << " imu_pose : " << SE3ToEigen(imu_pose) << std::endl;
    //std::cout << " imu_pose so3().matrix() : " << (imu_pose).so3().matrix() << std::endl;
    //std::cout << " imu_pose translation()  : " << (imu_pose).translation() << std::endl;
    //std::cout << " lidar_pose_from_imu : " << lidar_pose_from_imu << std::endl;
    return EigenToSE3(lidar_pose_from_imu);
}

SE3 SensorCalib::LidarToImuMotion(const SE3& lidar_pose, const Eigen::Matrix4d& lidar_pose_in_imu) {
    Eigen::Matrix4d imu_pose_from_lidar = lidar_pose_in_imu * SE3ToEigen(lidar_pose) * lidar_pose_in_imu.inverse();
    return EigenToSE3(imu_pose_from_lidar);
}

}  // namespace sad